#include "as5600sensor.h"

#define AS5600_MAX 4096
#define AS5601_MASK  0XFFF
#define AS5600_Address  0x36
#define RAW_Angle_Hi    0x0C

AS5600Sensor::AS5600Sensor()
{
    m_cpr = AS5600_MAX;
}

AS5600Sensor::~AS5600Sensor()
{
}

int AS5600Sensor::getRawCount()
{
#define DATA_BUF_SIZE 2

    if(m_iic == nullptr)
        return -1;

    uint8_t buf[DATA_BUF_SIZE];
    bool ok = m_iic->Read(AS5600_Address, RAW_Angle_Hi, buf, DATA_BUF_SIZE);
    if(!ok)
        return -1;

    return (buf[0] << 8 | buf[1]) & AS5601_MASK;
    
#undef DATA_BUF_SIZE
}
